#pragma once

#include "bspline_opt.h"
#include <glib/g2/p2p_matcher.h>
#include <glib/g2/translation.h>
#include <glib/g2/rigid.h>
#include <glib/g2/rigid_scale.h>
#include <glib/g2/affine.h>

namespace outline {

///this class determines best global motion (inside class C) of a gspline
template <typename R>
template <class C>
class bspline_opt_base<R>::gspline {
public:
	gspline(bool closed, uint nodes,
		std::vector<junc_type> const & junc, 
		real_vector const * inertia = 0);
public: //formulate and solve overdetermined equations
	///bt is a point on the bezier-piece with time=t
	void eq(uint piece, real t, point bt, point goal);
	void eq_solve(controls<R> & ctrls);
private:
	bool closed_;
	uint nodes_;
	std::vector<junc_type> const & junc_;
	real_vector inertia_;
	g2::p2p_matcher<C> p2p_matcher;
};

template <typename R>
template <class C>
bspline_opt_base<R>::gspline<C>::gspline(bool closed, uint nodes,
	std::vector<junc_type> const & junc, real_vector const * inertia) :
	nodes_(nodes), junc_(junc)
{
	closed_ = closed;
	p2p_matcher.clear();
	uint s = nodes_;

	//copy inertia: tangents of flat nodes must have inertia=1,
	if (inertia)
		inertia_ = *inertia;
	else
		inertia_.resize(closed ? (s*3 + 1) : (s*3 - 2), 0);
	for (uint i = 0; i < s; ++i) {
		if (!closed && (i == 0 || i+1 == s))
			continue;
		//left & right tangents of smooth & symmetrical nodes must have equal inertia
		if (!is_smooth_or_symm(junc[i]))
			continue;
		uint l = (i == 0) ? 3*s-1 : 3*i-1;
		uint r = 3*i+1;
		inertia_[l] = inertia_[r] = real(0.5) * (inertia_[l] + inertia_[r]);
	}
	if (closed_)
		inertia_.back() = inertia_.front();
}

template <typename R>
template <class C>
inline void bspline_opt_base<R>::gspline<C>::eq(uint piece, real t, point bt, point goal)
{
	assert (piece*3 + 3 < inertia_.size());
	real u = 1 - t;
	p2p_matcher.match(bt, goal,
		(1 - inertia_[piece*3]) * u*u*(1+2*t) +
		//(1 - inertia_[piece*3 + 1]) * 3*t*u*u + //if nodes are fixed, no global motion
		//(1 - inertia_[piece*3 + 2]) * 3*t*t*u +
		(1 - inertia_[piece*3 + 3]) * t*t*(3-2*t));
}

template <typename R>
template <class C>
void bspline_opt_base<R>::gspline<C>::eq_solve(controls<R> & ctrls)
{
	assert (ctrls.size() <= inertia_.size());
	assert (ctrls.nodes() == nodes_);
	C transform;
  p2p_matcher.get_transform(transform);
	uint s = nodes_;

	for (uint i = 0; i < s; ++i) {
		uint c = 3*i;
		point old_node_center = ctrls[c];
		ctrls[c] = (1 - inertia_[c]) * transform(old_node_center) + inertia_[c] * old_node_center;
		if (closed_ || i > 0) {
			uint l = (i == 0) ? 3*s-1 : 3*i-1; 
			ctrls[l] = ctrls[c] + 
				(1 - inertia_[l]) * (transform(ctrls[l]) - transform(old_node_center)) + inertia_[l] * (ctrls[l] - old_node_center);
		}
		if (closed_ || i+1 < s) {
			uint r = 3*i+1;
			ctrls[r] = ctrls[c] + 
				(1 - inertia_[r]) * (transform(ctrls[r]) - transform(old_node_center)) + inertia_[r] * (ctrls[r] - old_node_center);
		}
	}
	if (closed_)
		ctrls.back() = ctrls.front();

	p2p_matcher.clear();
}

} //namespace outline
